Camera Calibration

In [3]:
import pickle
import cv2
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import glob
%matplotlib inline

images = glob.glob('./camera_cal/calibration*.jpg')
In [4]:
objpoints = []
imgpoints = []
objp = np.zeros((6*9,3),np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)

for fname in images:
    
    img = mpimg.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_RGB2GRAY)

    ret, corners = cv2.findChessboardCorners(gray, (9,6), None)

    if ret == True:
        imgpoints.append(corners)
        objpoints.append(objp)
    
        img = cv2.drawChessboardCorners(img, (9,6), corners, ret)
        plt.imshow(img)
In [5]:
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)

with open('CameraCalibration.p','wb') as f:
    pickle.dump([ret,mtx,dist,rvecs,tvecs],f)
In [6]:
with open('CameraCalibration.p','rb') as f:
    data = pickle.load(f)

mtx = data[1]
dist = data[2]
In [10]:
img = mpimg.imread('./camera_cal/calibration1.jpg')
undist = cv2.undistort(img, mtx, dist, None)

f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()

ax1.imshow(img)
ax1.set_title('Original Image', fontsize=40)

ax2.imshow(undist)
ax2.set_title('Undistorted Image', fontsize=40)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Functions used for generating binary images

In [11]:
def abs_sobel_thresh(img, orient='x', sobel_kernel=3, thresh=(0, 255)):
    
    # 1) Convert to grayscale
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    # 2) Take the derivative in x or y given orient = 'x' or 'y'
    if orient == 'x':
        sobel = cv2.Sobel(gray, cv2.CV_64F, 1, 0)
    elif orient =='y':
        sobel = cv2.Sobel(gray, cv2.CV_64F, 0, 1)
    else:
        print('There is an error in the orient value')    
    
    # 3) Take the absolute value of the derivative or gradient
    abs_sobel = np.absolute(sobel)
    
    # 4) Scale to 8-bit (0 - 255) then convert to type = np.uint8
    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
    # 5) Create a mask of 1's where the scaled gradient magnitude 
            # is > thresh_min and < thresh_max
    binary_output = np.zeros_like(scaled_sobel)
    binary_output[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1
    # 6) Return this mask as your binary_output image
    #binary_output = np.copy(img) # Remove this line
    return binary_output

def mag_thresh(image, sobel_kernel=3, mag_thresh=(0, 255)):
    
    # Apply the following steps to img
    # 1) Convert to grayscale
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    # 2) Take the gradient in x and y separately
    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0,ksize=sobel_kernel)
    sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1,ksize=sobel_kernel)
    # 3) Calculate the magnitude 
    abs_sobel = np.sqrt(np.square(sobelx)+np.square(sobely))
    # 4) Scale to 8-bit (0 - 255) and convert to type = np.uint8
    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
    # 5) Create a binary mask where mag thresholds are met
    binary_output = np.zeros_like(scaled_sobel)
    binary_output[(scaled_sobel >= mag_thresh[0]) & (scaled_sobel <= mag_thresh[1])] = 1
    # 6) Return this mask as your binary_output image
    #binary_output = np.copy(img) # Remove this line
    
    return binary_output
    

def dir_threshold(image, sobel_kernel=3, thresh=(0, np.pi/2)):
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    
    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0,ksize=sobel_kernel)
    sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1,ksize=sobel_kernel)
    
    abs_sobelx = np.absolute(sobelx)
    abs_sobely = np.absolute(sobely)
    dir_sobel = np.arctan2(abs_sobely,abs_sobelx)
    
       
    binary_output = np.zeros_like(dir_sobel)
    binary_output[(dir_sobel >= thresh[0]) & (dir_sobel <= thresh[1])] = 1
    binary_output = np.uint8(binary_output)
    
    return binary_output

def hls_select(img,channel = 's', thresh=(0, 255)):
    # 1) Convert to HLS color space
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
    H = hls[:,:,0]
    L = hls[:,:,1]
    S = hls[:,:,2]
    if channel == 's':
        # 2) Apply a threshold to the S channel
        binary_output = np.zeros_like(S)
        binary_output[(S > thresh[0]) & (S <= thresh[1])] = 1
    elif channel == 'h':
        binary_output = np.zeros_like(H)
        binary_output[(H > thresh[0]) & (H <= thresh[1])] = 1
    elif channel == 'l':
        binary_output = np.zeros_like(L)
        binary_output[(L > thresh[0]) & (L <= thresh[1])] = 1
    else:
        print('There is an error in the channel value')
    # 3) Return a binary image of threshold result
    #binary_output = np.copy(img) # placeholder line
    return binary_output
def hsv_select(img,channel = 's', thresh=(0, 255)):
    # 1) Convert to HLS color space
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)
    H = hls[:,:,0]
    S = hls[:,:,1]
    V = hls[:,:,2]
    if channel == 's':
        # 2) Apply a threshold to the S channel
        binary_output = np.zeros_like(S)
        binary_output[(S > thresh[0]) & (S <= thresh[1])] = 1
    elif channel == 'h':
        binary_output = np.zeros_like(H)
        binary_output[(H > thresh[0]) & (H <= thresh[1])] = 1
    elif channel == 'v':
        binary_output = np.zeros_like(V)
        binary_output[(V > thresh[0]) & (V <= thresh[1])] = 1
    else:
        print('There is an error in the channel value')
    # 3) Return a binary image of threshold result
    #binary_output = np.copy(img) # placeholder line
    return binary_output

Functions used in the lane finding project

In [333]:
import math

def grayscale(img):
    """Applies the Grayscale transform
    This will return an image with only one color channel
    but NOTE: to see the returned image as grayscale
    (assuming your grayscaled image is called 'gray')
    you should call plt.imshow(gray, cmap='gray')"""
    return cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    # Or use BGR2GRAY if you read an image with cv2.imread()
    # return cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
def canny(img, low_threshold, high_threshold):
    """Applies the Canny transform"""
    return cv2.Canny(img, low_threshold, high_threshold)

def gaussian_blur(img, kernel_size,sigma = 2):
    """Applies a Gaussian Noise kernel"""
    return cv2.GaussianBlur(img, (kernel_size, kernel_size), sigma)

def region_of_interest(img, vertices):
    """
    Applies an image mask.
    
    Only keeps the region of the image defined by the polygon
    formed from `vertices`. The rest of the image is set to black.
    """
    #defining a blank mask to start with
    mask = np.zeros_like(img)   
    
    #defining a 3 channel or 1 channel color to fill the mask with depending on the input image
    if len(img.shape) > 2:
        channel_count = img.shape[2]  # i.e. 3 or 4 depending on your image
        ignore_mask_color = (255,) * channel_count
    else:
        ignore_mask_color = 255
        
    #filling pixels inside the polygon defined by "vertices" with the fill color    
    cv2.fillPoly(mask, vertices, ignore_mask_color)
    
    #returning the image only where mask pixels are nonzero
    masked_image = cv2.bitwise_and(img, mask)
    return masked_image


def draw_lines(img, lines, color=[255, 0, 0], thickness=2):
    """
    NOTE: this is the function you might want to use as a starting point once you want to 
    average/extrapolate the line segments you detect to map out the full
    extent of the lane (going from the result shown in raw-lines-example.mp4
    to that shown in P1_example.mp4).  
    
    Think about things like separating line segments by their 
    slope ((y2-y1)/(x2-x1)) to decide which segments are part of the left
    line vs. the right line.  Then, you can average the position of each of 
    the lines and extrapolate to the top and bottom of the lane.
    
    This function draws `lines` with `color` and `thickness`.    
    Lines are drawn on the image inplace (mutates the image).
    If you want to make the lines semi-transparent, think about combining
    this function with the weighted_img() function below
    """
    for line in lines:
        for x1,y1,x2,y2 in line:
            cv2.line(img, (x1, y1), (x2, y2), color, thickness)

# hough_lines modified to return also the lines it calculates as an array
def hough_lines(img, rho, theta, threshold, min_line_len, max_line_gap):
    """
    `img` should be the output of a Canny transform.
        
    Returns an image with hough lines drawn.
    """
    lines = cv2.HoughLinesP(img, rho, theta, threshold, np.array([]), minLineLength=min_line_len, maxLineGap=max_line_gap)
    line_img = np.zeros((img.shape[0], img.shape[1], 3), dtype=np.uint8)
    draw_lines(line_img, lines)
    return line_img, lines

# Python 3 has support for cool math symbols.

def weighted_img(img, initial_img, α=0.8, β=1., λ=0.):
    """
    `img` is the output of the hough_lines(), An image with lines drawn on it.
    Should be a blank image (all black) with lines drawn on it.
    
    `initial_img` should be the image before any processing.
    
    The result image is computed as follows:
    
    initial_img * α + img * β + λ
    NOTE: initial_img and img must be the same shape!
    """
    return cv2.addWeighted(initial_img, α, img, β, λ)

# function to avoid division by zero
def safe_div(x,y):
    if y == 0:
        return 0
    return x / y

def average_lines(lines,imshape,filt_coeff=1,height_coeff=0.7):
    """
    This function gets the lines provided by hough transform as input. 
    The lines are into two groups, one group contains lines on the left side of car 
    and the other contains lines on the right side of the car. The group is filtered
    based on the mean and the average parameters of the lines are calculated. This is 
    used to draw the extrapolated lines on the image
        
    """
    lines = lines[:,0,:]    
    lines_left = []
    lines_right = []       
    
    for x1,y1,x2,y2 in lines:
        """
         safe_div function is to calculate slope returns zero slope in case of 
         line parallel to yaxis. Though it is wrong these lines are not interesting 
         and it will be ignored                  
        """
                
        slope = safe_div((y2-y1),(x2-x1)) 
        x_intercept = y1-slope*x1
        #x0 = float((y0-x_intercept)/slope)
        if slope < 0:  
            lines_left.append([slope,x_intercept])            
        elif slope > 0:
            lines_right.append([slope,x_intercept]) 
            
    left_lines = np.asarray(lines_left)
    right_lines = np.asarray(lines_right)       
    
    mean_left = np.mean(left_lines,axis=0)
    mean_right = np.mean(right_lines,axis=0)
    
    filt_left_lines = []
    filt_right_lines =  []
    
    for line in lines_left:
        diff = math.fabs((mean_left[0] - line[0])/mean_left[0])        
        if diff < filt_coeff:
            filt_left_lines.append(line)
    for line in lines_right:
        diff = math.fabs((mean_right[0]- line[0])/mean_right[0])
        if diff < filt_coeff:
            filt_right_lines.append(line)
            
    filt_left_lines = np.array(filt_left_lines)
    filt_right_lines = np.array(filt_right_lines)
    
    if len(filt_left_lines)>0:
        mean_left = np.mean(filt_left_lines,axis=0)
    if len(filt_right_lines)>0:
        mean_right = np.mean(filt_right_lines,axis=0)
    
    y1 = imshape[0]*0.95
    y2 = imshape[0]*height_coeff  
    
    x1_left = (y1-mean_left[1])/mean_left[0]
    x2_left = (y2-mean_left[1])/mean_left[0]
    
    x1_right = (y1-mean_right[1])/mean_right[0]
    x2_right = (y2-mean_right[1])/mean_right[0]
    
    mean_lines = np.array(([[[x1_left,y1,x2_left,y2]],[[x1_right,y1,x2_right,y2]]]),dtype=np.int32)
    mean_lines_img = np.zeros((imshape[0], imshape[1], 3), dtype=np.uint8)
    draw_lines(mean_lines_img, mean_lines,color=[255, 0, 0], thickness=7)
    
    lane_lines = np.array([[x1_left,y1],[x2_left,y2],[x2_right,y2],[x1_right,y1]],dtype = np.int32)
    
    return mean_lines_img,lane_lines

def correctPerspectiveTransform(ploty,left_fit,right_fit,src,dst,Minv):
    
    y1 = dst[0][1]
    y2 = dst[1][1]
    
    slope_left = 2*left_fit[0]*y1+left_fit[1]
    slope_right = 2*right_fit[0]*y1+right_fit[1]
    
    offset = 0
    
    if abs(slope_left) <0.1 and abs(slope_right)<0.1:
        
        src = src
        dst = dst
        
    elif abs(slope_left) > abs(slope_right): 
        
        # correcting left lane line in perspective transform since the slope(dx/dy) of left is not zero
        # It is best to correct perspective transform only on one lane if both the lanes are corrected they counteract
        # Finding equation of line using old points in the source  matrix
        slope_src_right = (src[2][0]-src[3][0])/(src[2][1]-src[3][1])
        const_right =src[3][0] - slope_src_right*src[3][1]
        # correcting for center of the lane using curve fit
        x1_right = np.polyval(right_fit,y1)                       
        x1_left = np.polyval(left_fit,y1)
        
        dst = np.array([[x1_left,y1],[x1_left,y2],[x1_right,y2],[x1_right,y1]],dtype = 'int32')
        
        x2_left_dst = np.polyval(left_fit,y2)
        src_left_2 = np.dot(Minv,[x2_left_dst,y2,1])
        src_left_2 = src_left_2/src_left_2[2]
        
        x2_right = src_left_2[1]*slope_src_right + const_right
        
        src = np.array([[x1_left,y1],[src_left_2[0],src_left_2[1]],[x2_right,src_left_2[1]],[x1_right,y1]],dtype = 'int32')            

    elif abs(slope_right) > abs(slope_left):
        
        # correcting left lane line in perspective transform since the slope(dx/dy) of left is not zero
        # It is best to correct perspective transform only on one lane if both the lanes are corrected they counteract
        # Finding equation of line using old points in the source  matrix
        slope_src_left = (src[1][0]-src[0][0])/(src[1][1]-src[0][1])
        const_left =src[0][0] - slope_src_left*src[0][1]
        # correcting for center of the lane using curve fit
        x1_right = np.polyval(right_fit,y1)                       
        x1_left = np.polyval(left_fit,y1)
        
        dst = np.array([[x1_left,y1],[x1_left,y2],[x1_right,y2],[x1_right,y1]],dtype = 'int32')
        
        x2_right_dst = np.polyval(right_fit,y2)
        src_right_3 = np.dot(Minv,[x2_right_dst,y2,1])
        src_right_3 = src_right_3/src_right_3[2]
        
        x2_left = src_right_3[1]*slope_src_left + const_left
        
        src = np.array([[x1_left,y1],[x2_left,src_right_3[1]],[src_right_3[0],src_right_3[1]],[x1_right,y1]],dtype = 'int32')      
           
      
    
    return src,dst
    
    
In [334]:
img = mpimg.imread('./test_images/test2.jpg')

undist = cv2.undistort(img, mtx, dist, None)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()

ax1.imshow(img)
ax1.set_title('Original Image', fontsize=40)

ax2.imshow(undist)
ax2.set_title('Undistorted Image', fontsize=40)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
In [365]:
ksize = 5
gradx = abs_sobel_thresh(undist, orient='x', sobel_kernel=ksize, thresh=(20, 100))
grady = abs_sobel_thresh(undist, orient='y', sobel_kernel=ksize, thresh=(30, 100))
mag_binary = mag_thresh(undist, sobel_kernel=ksize, mag_thresh=(70, 150))
dir_binary = dir_threshold(undist, sobel_kernel=ksize, thresh=(np.pi*30./180,np.pi*80./180))
s_binary = hls_select(undist,channel = 's', thresh=(100, 255))
l_binary = hls_select(undist,channel = 'l', thresh=(50, 255))
v_binary = hsv_select(undist,channel = 'v', thresh=(150, 255))


combined_gradxy = np.zeros_like(dir_binary)
combined_gradxy[((gradx == 1) & (grady == 1))] = 1

combined_mag_dir = np.zeros_like(dir_binary)
combined_mag_dir[((mag_binary == 1) & (v_binary==1))] = 1

combined_hls = np.zeros_like(dir_binary)
combined_hls[(s_binary == 1)&(l_binary==1)] = 1

combined = np.zeros_like(dir_binary)
combined[(combined_gradxy== 1)|(combined_mag_dir== 1)|(combined_hls== 1)] = 1
#combined[(gradx== 1)|(s_binary== 1)] = 1

# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()

ax1.imshow(img)
ax1.set_title('Original Image', fontsize=40)

ax2.imshow(combined)
ax2.set_title('Pipeline Result', fontsize=40)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
In [336]:
combined.max()
Out[336]:
1
In [337]:
imshape = img.shape
width = imshape[1]
height = imshape[0]
#combined = np.uint8(combined)*255
height_prm = 0.11
width_prm = 0.06
vertices = np.array([[(width_prm*width,height*(1-height_prm/2)),(width*(0.5-width_prm),height*(0.5+height_prm)),
                      (width*(0.5+width_prm),height*(0.5+height_prm)),(width*(1-width_prm),height*(1-height_prm/2))]], dtype=np.int32)
# masking the image from canny edge algorithm
masked_edges = region_of_interest(combined,vertices)

rho = 2 # distance resolution in pixels of the Hough grid
theta = np.pi/180 # angular resolution in radians of the Hough grid
threshold = 50     # minimum number of votes (intersections in Hough grid cell)
min_line_length = 40 #minimum number of pixels making up a line
max_line_gap = 20    # maximum gap in pixels between connectable line segments
[line_img,lines] = hough_lines(masked_edges,rho,theta,threshold,min_line_length,max_line_gap)


mean_line_img,lane_lines =  average_lines(lines,imshape,height_coeff=0.72)
lane_mark = weighted_img(mean_line_img,img,0.8,1,0)
plt.imshow(lane_mark)
Out[337]:
<matplotlib.image.AxesImage at 0x2562ca3b2e8>
In [338]:
copy_image = np.copy(img)


offset = 0
arrOffset = np.array([[-offset,0],[-offset,0],[offset,0],[offset,0]],dtype='int32')
src = lane_lines + arrOffset
#src = np.array([[ 241,  684],[ 497,  518],[812,518],[1108,  684]],dtype = np.int32)
#src = np.array([[220,720],[540,500],[820,500],[1190,720]],dtype = np.int32)
#src = np.array([[270,720],[590,500],[780,500],[1150,720]],dtype = np.int32)

x1_dst = src[0][0]
x2_dst = src[3][0]

y1_dst = src[0][1]
y2_dst = src[1][1]

dst = np.array([[x1_dst,y1_dst],[x1_dst,y2_dst],[x2_dst,y2_dst],[x2_dst,y1_dst]],dtype='int32')

copy_img = cv2.polylines(copy_image,[src],True,(255,0,0),lineType=8,thickness= 3)
plt.imshow(copy_img)
Out[338]:
<matplotlib.image.AxesImage at 0x2562ca49e48>
In [339]:
src_found = src
src
Out[339]:
array([[ 345,  684],
       [ 515,  518],
       [ 814,  518],
       [1162,  684]])
In [340]:
# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()

ax1.imshow(combined)
ax1.set_title('Pipeline Image', fontsize=40)

ax2.imshow(masked_edges)
ax2.set_title('Masked Edges', fontsize=40)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
In [367]:
img_size = (img.shape[1],img.shape[0])
#dst = np.array([[230,740],[230,500],[1230,500],[1230,720]],dtype = np.int32)
M = cv2.getPerspectiveTransform(np.float32(src),np.float32(dst))
Minv = cv2.getPerspectiveTransform(np.float32(dst),np.float32(src))
image_warped = cv2.warpPerspective(copy_image,M,img_size,flags=cv2.INTER_LINEAR)
plt.imshow(image_warped)
Out[367]:
<matplotlib.image.AxesImage at 0x25629fd7eb8>
In [342]:
src
Out[342]:
array([[ 345,  684],
       [ 515,  518],
       [ 814,  518],
       [1162,  684]])
In [343]:
Minv
Out[343]:
array([[  1.22872859e-01,  -7.86243706e-01,   5.37790695e+02],
       [ -3.21964677e-15,  -5.41384244e-01,   4.54351859e+02],
       [ -6.28837260e-18,  -1.28234962e-03,   1.00000000e+00]])
In [344]:
dst
Out[344]:
array([[ 345,  684],
       [ 345,  518],
       [1162,  518],
       [1162,  684]])
In [345]:
x = np.dot(Minv,np.array([251,503,1]))
x/x[2]
           
Out[345]:
array([ 487.77989009,  512.80786934,    1.        ])
In [346]:
src
Out[346]:
array([[ 345,  684],
       [ 515,  518],
       [ 814,  518],
       [1162,  684]])
In [347]:
binary_warped = image_warped
# Assuming you have created a warped binary image called "binary_warped"
# Take a histogram of the bottom half of the image
histogram = np.sum(binary_warped[int(binary_warped.shape[0]/2):,:], axis=0)
# Create an output image to draw on and  visualize the result
out_img = np.dstack((binary_warped, binary_warped, binary_warped))
# Find the peak of the left and right halves of the histogram
# These will be the starting point for the left and right lines
midpoint = np.int(histogram.shape[0]/2)
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint

# Choose the number of sliding windows
nwindows = 9
# Set height of windows
window_height = np.int(binary_warped.shape[0]/nwindows)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Current positions to be updated for each window
leftx_current = leftx_base
rightx_current = rightx_base
# Set the width of the windows +/- margin
margin = 100
# Set minimum number of pixels found to recenter window
minpix = 50
In [348]:
plt.plot(histogram)
Out[348]:
[<matplotlib.lines.Line2D at 0x25629976c50>]
In [349]:
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# Step through the windows one by one
for window in range(nwindows):
    # Identify window boundaries in x and y (and right and left)
    win_y_low = binary_warped.shape[0] - (window+1)*window_height
    win_y_high = binary_warped.shape[0] - window*window_height
    win_xleft_low = leftx_current - margin
    win_xleft_high = leftx_current + margin
    win_xright_low = rightx_current - margin
    win_xright_high = rightx_current + margin
    # Draw the windows on the visualization image
    cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),
    (0,255,0), 3) 
    cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),
    (0,255,0), 3) 
    # Identify the nonzero pixels in x and y within the window
    good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & 
    (nonzerox >= win_xleft_low) &  (nonzerox < win_xleft_high)).nonzero()[0]
    good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & 
    (nonzerox >= win_xright_low) &  (nonzerox < win_xright_high)).nonzero()[0]
    # Append these indices to the lists
    left_lane_inds.append(good_left_inds)
    right_lane_inds.append(good_right_inds)
    # If you found > minpix pixels, recenter next window on their mean position
    if len(good_left_inds) > minpix:
        leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
    if len(good_right_inds) > minpix:        
        rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
In [350]:
# Concatenate the arrays of indices
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)

# Extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds] 
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds] 

# Fit a second order polynomial to each
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
# Generate x and y values for plotting
ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]

out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
plt.imshow(out_img)
plt.plot(left_fitx, ploty, color='yellow')
plt.plot(right_fitx, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
Out[350]:
(720, 0)
In [351]:
# Assume you now have a new warped binary image 
# from the next frame of video (also called "binary_warped")
# It's now much easier to find line pixels!
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
margin = 100
left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + 
left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) + 
left_fit[1]*nonzeroy + left_fit[2] + margin))) 

right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + 
right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) + 
right_fit[1]*nonzeroy + right_fit[2] + margin)))  

# Again, extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds] 
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
# Fit a second order polynomial to each
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
# Generate x and y values for plotting
ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
In [352]:
# Create an image to draw on and an image to show the selection window
out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
window_img = np.zeros_like(out_img)
# Color in left and right line pixels
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]

# Generate a polygon to illustrate the search window area
# And recast the x and y points into usable format for cv2.fillPoly()
left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, 
                              ploty])))])
left_line_pts = np.hstack((left_line_window1, left_line_window2))
right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, 
                              ploty])))])
right_line_pts = np.hstack((right_line_window1, right_line_window2))

# Draw the lane onto the warped blank image
cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
plt.imshow(result)
plt.plot(left_fitx, ploty, color='yellow')
plt.plot(right_fitx, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
Out[352]:
(720, 0)
In [353]:
# Read in a thresholded image
warped = image_warped
# window settings
window_width = 50 
window_height = 80 # Break image into 9 vertical layers since image height is 720
margin = 100 # How much to slide left and right for searching

def window_mask(width, height, img_ref, center,level):
    output = np.zeros_like(img_ref)
    output[int(img_ref.shape[0]-(level+1)*height):int(img_ref.shape[0]-level*height),
           max(0,int(center-width/2)):min(int(center+width/2),img_ref.shape[1])] = 1
    return output

def find_window_centroids(image, window_width, window_height, margin):
    
    window_centroids = [] # Store the (left,right) window centroid positions per level
    window = np.ones(window_width) # Create our window template that we will use for convolutions
    
    # First find the two starting positions for the left and right lane by using np.sum to get the vertical image slice
    # and then np.convolve the vertical image slice with the window template 
    
    # Sum quarter bottom of image to get slice, could use a different ratio
    l_sum = np.sum(image[int(3*image.shape[0]/4):,:int(image.shape[1]/2)], axis=0)
    l_center = np.argmax(np.convolve(window,l_sum))-window_width/2
    r_sum = np.sum(image[int(3*image.shape[0]/4):,int(image.shape[1]/2):], axis=0)
    r_center = np.argmax(np.convolve(window,r_sum))-window_width/2+int(image.shape[1]/2)
    
    # Add what we found for the first layer
    window_centroids.append((l_center,r_center))
    
    # Go through each layer looking for max pixel locations
    for level in range(1,(int)(image.shape[0]/window_height)):
	    # convolve the window into the vertical slice of the image
	    image_layer = np.sum(image[int(image.shape[0]-(level+1)*window_height):int(image.shape[0]-level*window_height),:], axis=0)
	    conv_signal = np.convolve(window, image_layer)
	    # Find the best left centroid by using past left center as a reference
	    # Use window_width/2 as offset because convolution signal reference is at right side of window, not center of window
	    offset = window_width/2
	    l_min_index = int(max(l_center+offset-margin,0))
	    l_max_index = int(min(l_center+offset+margin,image.shape[1]))
	    l_center = np.argmax(conv_signal[l_min_index:l_max_index])+l_min_index-offset
	    # Find the best right centroid by using past right center as a reference
	    r_min_index = int(max(r_center+offset-margin,0))
	    r_max_index = int(min(r_center+offset+margin,image.shape[1]))
	    r_center = np.argmax(conv_signal[r_min_index:r_max_index])+r_min_index-offset
	    # Add what we found for that layer
	    window_centroids.append((l_center,r_center))

    return window_centroids

window_centroids = find_window_centroids(warped, window_width, window_height, margin)

# If we found any window centers
if len(window_centroids) > 0:

    # Points used to draw all the left and right windows
    l_points = np.zeros_like(warped)
    r_points = np.zeros_like(warped)

    # Go through each level and draw the windows 	
    for level in range(0,len(window_centroids)):
        # Window_mask is a function to draw window areas
	    l_mask = window_mask(window_width,window_height,warped,window_centroids[level][0],level)
	    r_mask = window_mask(window_width,window_height,warped,window_centroids[level][1],level)
	    # Add graphic points from window mask here to total pixels found 
	    l_points[(l_points == 255) | ((l_mask == 1) ) ] = 255
	    r_points[(r_points == 255) | ((r_mask == 1) ) ] = 255
        
    nonzero_left = l_points.nonzero()
    nonzero_lefty = np.array(nonzero_left[0])
    nonzero_leftx = np.array(nonzero_left[1])
    
    nonzero_right = r_points.nonzero()
    nonzero_righty = np.array(nonzero_right[0])
    nonzero_rightx = np.array(nonzero_right[1])
    
    left_conv_fit = np.polyfit(nonzero_lefty, nonzero_leftx, 2)
    right_conv_fit = np.polyfit(nonzero_righty, nonzero_rightx, 2)
    
    left_conv_fitx = left_conv_fit[0]*ploty**2 + left_conv_fit[1]*ploty + left_conv_fit[2]
    right_conv_fitx = right_conv_fit[0]*ploty**2 + right_conv_fit[1]*ploty + right_conv_fit[2]

    # Draw the results
    template = np.array(r_points+l_points,np.uint8) # add both left and right window pixels together
    zero_channel = np.zeros_like(template) # create a zero color channel
    template = np.array(cv2.merge((zero_channel,template,zero_channel)),np.uint8) # make window pixels green
    warpage= np.dstack((warped, warped, warped))*255 # making the original road pixels 3 color channels
    output = cv2.addWeighted(warpage, 1, template, 0.5, 0.0) # overlay the orignal road image with window results
 
# If no window centers found, just display orginal road image
else:
    output = np.array(cv2.merge((warped,warped,warped)),np.uint8)

# Display the final results
plt.imshow(output)
plt.title('window fitting results')
plt.plot(left_conv_fitx, ploty, color='yellow')
plt.plot(right_conv_fitx, ploty, color='yellow')
plt.show()
In [354]:
left_fit
Out[354]:
array([ -4.65044532e-04,   3.91134502e-01,   2.81060980e+02])
In [355]:
road_center=(right_fitx[719]-left_fitx[719])/2+left_fitx[719]
road_center
Out[355]:
717.97502055178779
In [356]:
car_center = output.shape[1]/2
difference_pixels = (car_center -road_center)
difference_meters = difference_pixels*3.7/700
print(difference_meters)
-0.412153680059
In [357]:
y_eval = np.max(ploty)
# Define conversions in x and y from pixels space to meters
ym_per_pix = 30/720 # meters per pixel in y dimension
xm_per_pix = 3.7/700 # meters per pixel in x dimension

# Fit new polynomials to x,y in world space
left_fit_cr = np.polyfit(ploty*ym_per_pix, left_fitx*xm_per_pix, 2)
right_fit_cr = np.polyfit(ploty*ym_per_pix, right_fitx*xm_per_pix, 2)

# Calculate the new radii of curvature
left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
right_curverad = ((1 + (2*right_fit_cr[0]*y_eval*ym_per_pix + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])

# Now our radius of curvature is in meters
print(left_curverad, 'm', right_curverad, 'm')
353.799053642 m 444.532060835 m
In [358]:
# Create an image to draw the lines on
color_warp = np.zeros_like(undist).astype(np.uint8)
#color_warp = np.dstack((warp_zero, warp_zero, warp_zero))

# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))

# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))




# Warp the blank back to original image space using inverse perspective matrix (Minv)
newwarp = cv2.warpPerspective(color_warp, Minv, (undist.shape[1], undist.shape[0])) 
# Combine the result with the original image
result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)

curvature = min(left_curverad,right_curverad)
text1 = 'Radius of the curvature of the inside lane: '+ str(curvature) + ' m'
text2 = 'Lateral distance of the car from road center: ' + str(difference_meters)+ ' m'
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(result,text1,(10,60), font, 1,(255,255,255),2)
cv2.putText(result,text2,(10,120), font, 1,(255,255,255),2)

f, (ax1) = plt.subplots(1, figsize=(24, 9))
f.tight_layout()
ax1.imshow(result)
Out[358]:
<matplotlib.image.AxesImage at 0x25627eb35c0>